#include "motion.h"



Motion::Motion(Location currLoc)
{
	currLoc_ = currLoc;
	drivec = new Drivecontrol(false);
	log = new Log("Motion");

	//vars
	deviX_ = 1;
	deviY_ = 1;
	deviR_ = 10;
	normSpeed_ = 20;
}

Motion::~Motion()
{
	delete drivec;
	delete log;
}



void Motion::moveStrait( Location targetLoc )
{
	//we veronderstellen dat we niet moeten draaien. updateLocation zal zeggen wanneer we er zijn
	drivec->initRotate( Motion::approachAngle(targetLoc), false );
	drivec->initDriveForward(normSpeed_);
}



void Motion::rotate( Location targetLoc )
{

}

int Motion::catchBall ( Location targetLoc )
{

}

void Motion::kickBall ( Location targetLoc )
{

}

float Motion::approachAngle(Location dest ){

	float dx,dy,r;

	dx = dest.getX() - currLoc_.getX();
	dy = dest.getY() - currLoc_.getY();
	r = currLoc_.getRotation();

	if(dy > 0){
		if( dx > 0 ){
			//first quadrant
			return 180/M_PI * atan(dy/dx) - r;
		}
		if( dx < 0){
			//second quadrant
			dx = dx * -1;
			return 180/M_PI * atan(dy/dx) - r + 90;
		}
		if( dx == 0){
			//above us
			return 90 - r;
		}
	}

	if(dy < 0 ){
		if( dx > 0 ){
			//forth quadrant
			dy = dy * -1;
			return 180/M_PI * atan(dy/dx) -r + 270;
		}
		if( dx < 0){
			//third quadrant
			dy = dy * -1;
			dx = dx * -1;
			return 180/M_PI * atan(dy/dx) -r + 180;
		}
		if( dx == 0){
			//below us
			return 180 -r;
		}
	}

	if(dy == 0 ){
		if( dx>0 ){
			//right of us
			return 0;
		}
		if( dx<0 ){
			//left of us
			return 180;
		}
		//dy==0 && dx==0
	}
	return 0;
}
void Motion::updateLocation(Location currLoc){

	currLoc_ = currLoc;
	

	if(state_ == mStrait ){
		if( Motion::arrived() ){
			drivec->stopManeuver();
			state_ = stopped;
		}else{
			//check if re-orientation is necessary
			float nHeading = Motion::approachAngle( targetLoc_ );
			if( deviR_ <= abs( nHeading ) ){
				log->stuurf("Correction occurred. Rotation is '%f' with distance '%f'.", nHeading, currLoc_.getDistance(targetLoc) );
				drivec->initRotate( nHeading, true );
				drivec->initDriveForward(normSpeed_);
			}
		}
	}
}
bool Motion::arrived(){

	int dx,dy;
	dx = abs( currLoc_.getX() - targetLoc_.getX());
	dy = abs( currLoc_.getY() - targetLoc_.getY());
	if( (deviX_ >= dx) && (deviY_ >= dy) ) return true;
	return false;
}
